#include "MyRobot.h"

/**
 * This file includes the DisplaySmartDashboard() method, which
 * is called whenever you call it (called once every Operator
 * Control loop cycle. Any and all sensors that need be added to
 * the Smart Dashboard should be included here, and will update
 * automatically whenever this method is called.
 */

void RobotDemo::UpdateSmartDashboard(bool testing)
{
	if (!testing)	//use for normal robot operations
	{
		//Use the following to add/update controls to the SmartDashboard
		//SmartDashboard::PutBoolean(...) <-- true/false control
		//SmartDashboard::PutNumber(...)  <-- numerical value control
		//SmartDashboard::PutString(...)  <-- text control
		//SmartDashboard::PutData(...)	  <-- button to execute a provided method
		
		/*PID Stuff
		SmartDashboard::PutNumber("GearToothPIDFirst", RPMGearToothFirst.PIDGet());
		SmartDashboard::PutNumber("GearToothPIDSecond", RPMGearToothSecond.PIDGet());
		SmartDashboard::PutNumber("PIDfirst", PIDfirst.Get());
		*/
		
		//Targeting Information
		/*
		targetX = (rcvData & TX_NOTARGET) ? "NO TARGET!" :
			(rcvData & TX_LEFT) ? "GO RIGHT!" : 
			(rcvData & TX_RIGHT) ? "GO LEFT!" :
			(rcvData & TX_ONTARGET) ? "GOLDEN!" : "error";
		targetY = (rcvData & TY_NOTARGET) ? "NO TARGET!" :
			(rcvData & TY_FORWARD) ? "GO BACKWARD!" : 
			(rcvData & TY_BACKWARD) ? "GO FORWARD!" :
			(rcvData & TY_ONTARGET) ? "GOLDEN!" : "error";
		
		SmartDashboard::PutString("Targeting X", targetX);
		SmartDashboard::PutString("Targeting Y", targetY);
		dispAimX.setData(targetX);
		dispAimY.setData(targetY);
		*/
	}
	else		//use for the testing mode
	{
		//PID Control Tuning
		//LiveWindow::GetInstance()->AddActuator("Shooters", "First", &shooterFirst);
		//LiveWindow::GetInstance()->AddActuator("Shooters", "Second", &shooterSecond);
		//LiveWindow::GetInstance()->AddActuator("Shooters", "PID", &PIDfirst);
		//LiveWindow::GetInstance()->AddSensor("Shooters", "Encoder", &encoderFirst);
		//LiveWindow::GetInstance()->AddSensor("Shooters", "GearTooth", &RPMGearToothFirst);
	}
}
